package zjut.raymond.witdemo.sensor;

import android.hardware.SensorManager;
import android.util.Log;

import zjut.raymond.witdemo.algorithm.MovingAverageFilter;
import zjut.raymond.witdemo.algorithm.STDCalculator;
import zjut.raymond.witdemo.algorithm.StepCallback;
import zjut.raymond.witdemo.algorithm.Vector3;
import zjut.raymond.witdemo.algorithm.Vector3Event;

public class SensorDataListener implements SensorDataObserver {
    private static final String TAG = "SensorDataListener";

    StepCallback stepCallback;

    //================以下为PDR相关参数============================
    private Vector3Event mAccV = new Vector3Event();
    private Vector3 linear_AccV = new Vector3();
    private Vector3Event mMagV = new Vector3Event();
    private Vector3Event mGyrV = new Vector3Event();

    // Window_size
    private static final int ACCELERATION_WINDOW = 10;
    private static final int MAGNETIC_FIELD_WINDOW = 20;
    private static final int STD_WINDOW = 20;
    private static final int ORIENTATION_WINDOW = 10;
    private MovingAverageFilter mAccelerometerFilter_X = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter mAccelerometerFilter_Y = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter mAccelerometerFilter_Z = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter linearAccelerometerFilter_X = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter linearAccelerometerFilter_Y = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter linearAccelerometerFilter_Z = new MovingAverageFilter(ACCELERATION_WINDOW);
    private MovingAverageFilter mMagneticFilter_X = new MovingAverageFilter(MAGNETIC_FIELD_WINDOW);
    private MovingAverageFilter mMagneticFilter_Y = new MovingAverageFilter(MAGNETIC_FIELD_WINDOW);
    private MovingAverageFilter mMagneticFilter_Z = new MovingAverageFilter(MAGNETIC_FIELD_WINDOW);
    private MovingAverageFilter mSinFilter = new MovingAverageFilter(ORIENTATION_WINDOW);
    private MovingAverageFilter mCosFilter = new MovingAverageFilter(ORIENTATION_WINDOW);

    private STDCalculator mAccSTDCalculator = new STDCalculator(STD_WINDOW);

    private static final double LOCAL_GRAVITY = 9.81d;
    private double accStandardDeviation = 0.0d;

    // Min/Max interval of two steps
    private static final int MIN_STEP_INTERVAL = 450;
    private static final int MIN_RUN_INTERVAL = 350;
    private static final int INT_STEP_INTERVAL = 600;
    private static final int MAX_STEP_INTERVAL = 1800;
    private float dynamicInterval = INT_STEP_INTERVAL;
    private float dynamicMinInterval = MIN_STEP_INTERVAL;

    // Date
    private long mLastStepTime = System.currentTimeMillis();

    private float K = 0.6f;
    private float dynamicStepLength;

    // Orientation
    private boolean hasOrientation = false;
    private boolean initState = false;
    private float[] rotationMatrix = new float[9];
    private float[] outR = new float[9];
    private float mOrientationResult;
    private float[] mOrientationValues = new float[3];// orientation angles from accel and magnet
    private float[] deltaRotationVector = new float[4];// copy the new gyro values into the gyro array convert the raw gyro
    private float[] gyroMatrix = new float[9]; // rotation matrix from gyro data
    private float[] gyroOrientation = new float[3]; // orientation angles from gyro matrix
    private float[] deltaRotationMatrix = new float[9]; // convert rotation vector into rotation matrix
    private float[] fusedOrientation = new float[3]; // final orientation angles from sensor fusion
    private float pitch;
    private float roll;

    // trigger acceleration of one step (ALWAYS >0)
    private static final float STEP_TRIGGER_ACCEL = 1.6f;
    private float maxSumAcc = 2.0f;
    private float minSumAcc = 1.0f;
    private static final float maxPeakThreshold = 2.0f;
    private static final float minPeakThreshold = 1.0f;

    //互补滤波器系数
    private static final float FILTER_COEFFICIENT = 0.95f;

    public static final float EPSILON = 0.000000001f;
    private static final float NS2S = 1.0f / 1000000000.0f;
    private float timestamp;

    public SensorDataListener() {
        initObjects();
    }

    private void initObjects() {
        gyroOrientation[0] = 0.0f;
        gyroOrientation[1] = 0.0f;
        gyroOrientation[2] = 0.0f;

        // Initialize gyroMatrix with identity matrix
        gyroMatrix[0] = 1.0f;
        gyroMatrix[1] = 0.0f;
        gyroMatrix[2] = 0.0f;
        gyroMatrix[3] = 0.0f;
        gyroMatrix[4] = 1.0f;
        gyroMatrix[5] = 0.0f;
        gyroMatrix[6] = 0.0f;
        gyroMatrix[7] = 0.0f;
        gyroMatrix[8] = 1.0f;

        // initialize weightMatrix;
//        weightMatrix[0] = 4.0f;
//        weightMatrix[1] = 2.0f;
//        weightMatrix[2] = 1.0f;
    }

    public void setStepCallback(StepCallback stepCallback) {
        this.stepCallback = stepCallback;
    }

    @Override
    public void onSensorChanged(MyEvent event) {
       // executorService.execute(() -> {
            switch (event.getType()) {
                case TYPE_ACCELEROMETER: {
                    Log.d(TAG, "onSensorChanged: 加速度：" + event.toString());
                    Log.d(TAG, "onSensorChanged: 旋转角：" + pitch + " " + roll);
                    //获取线性加速度
                    double x = event.x + Math.sin(roll) * Math.cos(pitch) * LOCAL_GRAVITY;
                    double y = event.y + Math.sin(pitch) * LOCAL_GRAVITY;
                    double z = event.z - Math.cos(roll) * Math.cos(pitch) * LOCAL_GRAVITY;
                    linear_AccV.x = linearAccelerometerFilter_X.filterProcess(x);
                    linear_AccV.y = linearAccelerometerFilter_Y.filterProcess(y);
                    linear_AccV.z = linearAccelerometerFilter_Z.filterProcess(z);
                    Log.d(TAG, "onSensorChanged: 线性加速度" + linear_AccV.getNorm());

                    //TODO:步态检测和步长估计
                    //滤波处理
                    mAccV.x = mAccelerometerFilter_X.filterProcess(event.x);
                    mAccV.y = mAccelerometerFilter_Y.filterProcess(event.y);
                    mAccV.z = mAccelerometerFilter_Z.filterProcess(event.z);
                    Log.d(TAG, "dealCallDatas: 加速度数据：滤波后:" + mAccV.toString());

                    double a_f = linear_AccV.getNorm();

                    Log.d(TAG, "inputData: 合加速度" + a_f);
                    accStandardDeviation = mAccSTDCalculator.getStd(a_f);

                    if (accStandardDeviation > 2.5) {
                        K = 0.75f;
                        dynamicMinInterval = MIN_RUN_INTERVAL;
                    } else {
                        K = 0.6f;
                        dynamicMinInterval = MIN_STEP_INTERVAL;
                        if (dynamicInterval < dynamicMinInterval)
                            dynamicInterval = MIN_STEP_INTERVAL;
                    }
                    if (a_f > maxSumAcc) {
                        maxSumAcc = (float) a_f;
                    } else if (a_f < minSumAcc) {
                        minSumAcc = (float) a_f;
                    }
                    // Step Detect
                    long currentTime = System.currentTimeMillis();
                    if (a_f > STEP_TRIGGER_ACCEL) {
                        long delta_t = currentTime - mLastStepTime;
                        Log.d(TAG, "onSensorChanged: delta_t :" + delta_t);
                        if (delta_t >= MAX_STEP_INTERVAL)
                            dynamicInterval = INT_STEP_INTERVAL;
                        else if (delta_t > dynamicInterval) {
                            //动态步长
                            dynamicStepLength = (float) (K * Math.sqrt(Math.sqrt(maxSumAcc - minSumAcc)));
                            Log.d(TAG, "onSensorChanged: 新的步伐：" + dynamicStepLength + " " + mOrientationResult);
                            stepCallback.onLocationChanged(dynamicStepLength, mOrientationResult);
                            maxSumAcc = maxPeakThreshold;
                            minSumAcc = minPeakThreshold;
                        } else if (delta_t <= dynamicInterval && delta_t > dynamicMinInterval)
                            dynamicInterval -= 50;
                        else return;
                        mLastStepTime = currentTime;
                        Log.d(TAG, "onSensorChanged: dynamicInterval: " + dynamicInterval);
                    }
                }
                break;
                case TYPE_MAGNETIC_FIELD: {
                    //TODO:磁力计航向估计
                    mMagV.x = mMagneticFilter_X.filterProcess(event.x);
                    mMagV.y = mMagneticFilter_Y.filterProcess(event.y);
                    mMagV.z = mMagneticFilter_Z.filterProcess(event.z);
                    Log.d(TAG, "dealCallDatas: 磁力计数据：" + mMagV.toString());
                    //加速度计+磁力计航向估计
                    if (mAccV.getNorm() != 0 && mMagV.getNorm() != 0) {
                        if (SensorManager.getRotationMatrix(rotationMatrix, null, mAccV.toFloatArray(), mMagV.toFloatArray())) {
                            //姿势1：手机+y轴竖直朝下，+z轴为前进方向
                            SensorManager.getOrientation(rotationMatrix, mOrientationValues);
                            pitch = mOrientationValues[1];
                            roll = mOrientationValues[2];
                            SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_X, SensorManager.AXIS_MINUS_Z, outR);
                            SensorManager.getOrientation(outR, mOrientationValues);

                            hasOrientation = true;
                            Log.d(TAG, "dealCallDatas: 航向角" + mOrientationValues[0] * 180 / Math.PI);
                        }
                    }
                }
                break;
                case TYPE_GYROSCOPE: {
                    //TODO:陀螺仪航向估计
                    // This timestep's delta rotation to be multiplied by the current rotation
                    // after computing it from the gyro sample data.
                    if (!hasOrientation) {
                        return;
                    }
                    // Initialization of the gyroscope based rotation matrix
                    if (!initState) {
                        gyroMatrix = matrixMultiplication(gyroMatrix, rotationMatrix);
                        initState = true;
                    }

                    if (timestamp != 0) {
                        final float dT = (event.timestamp - timestamp) * NS2S;
                        // Axis of the rotation sample, not normalized yet.
                        float axisX = event.x;
                        float axisY = event.y;
                        float axisZ = event.z;

                        // Calculate the angular speed of the sample
                        float omegaMagnitude = (float) Math.sqrt(Math.pow(axisX, 2) + Math.pow(axisY, 2) + Math.pow(axisZ, 2));

                        // Normalize the rotation vector if it's big enough to get the axis
                        // (that is, EPSILON should represent your maximum allowable margin of error)
                        if (omegaMagnitude > EPSILON) {
                            axisX /= omegaMagnitude;
                            axisY /= omegaMagnitude;
                            axisZ /= omegaMagnitude;
                        }

                        // Integrate around this axis with the angular speed by the timestep
                        // in order to get a delta rotation from this sample over the timestep
                        // We will convert this axis-angle representation of the delta rotation
                        // into a quaternion before turning it into the rotation matrix.
                        float thetaOverTwo = omegaMagnitude * dT / 2.0f;
                        float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
                        float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
                        deltaRotationVector[0] = sinThetaOverTwo * axisX;
                        deltaRotationVector[1] = sinThetaOverTwo * axisY;
                        deltaRotationVector[2] = sinThetaOverTwo * axisZ;
                        deltaRotationVector[3] = cosThetaOverTwo;
                    }
                    timestamp = event.timestamp;

                    // Get the rotation matrix from the gyroscope
                    SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);

                    // Apply the new rotation interval on the gyroscope based rotation matrix to form a composite rotation matrix.
                    // The product of two rotation matricies is a rotation matrix... Multiplication of rotation matrices
                    // corresponds to composition of rotations... Which in this case are the rotation matrix from the fused
                    // orientation and the rotation matrix from the current gyroscope outputs.
                    gyroMatrix = matrixMultiplication(gyroMatrix, deltaRotationMatrix);

                    // Get the gyroscope based orientation from the composite rotation matrix. This orientation will be fused via
                    // complementary filter with the orientation from the acceleration sensor and magnetic sensor.
                    SensorManager.getOrientation(gyroMatrix, gyroOrientation);


                    // Log.d(TAG, "onSensorChanged: gyroOrientation:" + gyroOrientation[0] * 180 / Math.PI + " " + gyroOrientation[1] * 180 / Math.PI + " " + gyroOrientation[2] * 180 / Math.PI);

                    calculateFusedOrientation();
                    //    Log.d(TAG, "onSensorChanged: gyroOrientation1:" + fusedOrientation[0] * 180 / Math.PI + " ori:" + mOrientationValues[0] * 180 / Math.PI);
                    newOrientation(fusedOrientation[0]);

                }
                break;
                default:
                    break;
            }
       // });
    }


    //  Multiply A by B
    private float[] matrixMultiplication(float[] A, float[] B) {
        float[] result = new float[9];

        result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
        result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
        result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

        result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
        result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
        result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

        result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
        result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
        result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

        return result;
    }

    /**
     * 将获得的新角度做滤波，之后更新UI图标
     *
     * @param radius 以弧度制表示的角度
     */
    private void newOrientation(double radius) {
        mSinFilter.filterProcess(Math.sin(radius));
        mCosFilter.filterProcess(Math.cos(radius));
        float deg = (float) Math.toDegrees(Math.asin(mSinFilter.get()));
        if (mSinFilter.get() > 0 && mCosFilter.get() < 0) deg = 180 - deg;
        if (mSinFilter.get() < 0 && mCosFilter.get() < 0) deg = -180 - deg;
        mOrientationResult = deg;
        Log.d(TAG, "newOrientation: 航向角：" + mOrientationResult);
        stepCallback.onOrientationChanged(mOrientationResult);
//
//        //TODO : 图标角度更新
//        Message msg = new Message();
//        msg.what = UPDATE_ORIENTATION_CODE;
//        Bundle b = new Bundle();
//        b.putDouble("newOrientation", mOrientationResult);
//        msg.setData(b);
//        MainActivity.sHandler.sendMessage(msg);
    }

//    private void newStep() {
//        //TODO:更新位置
//        Message msg = new Message();
//        msg.what = UPDATE_LOCATION_CODE;
//        Bundle b = new Bundle();
////        b.putDouble("stepLength", mStepLength);
//        b.putDouble("stepLength", dynamicStepLength);
//        b.putDouble("mOrientationResult", mOrientationResult);
//        msg.setData(b);
//        MainActivity.sHandler.sendMessage(msg);
//    }

    // Calculate the fused orientation.
    private void calculateFusedOrientation() {
        float oneMinusCoeff = (1.0f - FILTER_COEFFICIENT);
//        Log.d(TAG, "calculateFusedOrientation: oneMinusCoeff" + oneMinusCoeff);
//        Log.d(TAG, "calculateFusedOrientation: gyro:" + gyroOrientation[0] + " " + "mag_acc:" + mOrientationValues[0]);
        /*
         * Fix for 179° <--> -179° transition problem: Check whether one of the two orientation angles (gyro or
         * accMag) is negative while the other one is positive. If so, add 360° (2 * math.PI) to the negative value,
         * perform the sensor fusion, and remove the 360° from the result if it is greater than 180°. This stabilizes
         * the output in positive-to-negative-transition cases.
         */

        // azimuth
        if (gyroOrientation[0] < -0.5 * Math.PI && mOrientationValues[0] > 0.0) {
            fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) +
                    oneMinusCoeff * mOrientationValues[0]
            );
            fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
        } else if (mOrientationValues[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
            fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff *
                    (mOrientationValues[0] + 2.0 * Math.PI)
            );
            fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
        } else {
            fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * mOrientationValues[0];
        }

        // pitch
        if (gyroOrientation[1] < -0.5 * Math.PI && mOrientationValues[1] > 0.0) {
            fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) +
                    oneMinusCoeff * mOrientationValues[1]
            );
            fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
        } else if (mOrientationValues[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) {
            fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff *
                    (mOrientationValues[1] + 2.0 * Math.PI)
            );
            fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
        } else {
            fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * mOrientationValues[1];
        }

        // roll
        if (gyroOrientation[2] < -0.5 * Math.PI && mOrientationValues[2] > 0.0) {
            fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) +
                    oneMinusCoeff * mOrientationValues[2]
            );
            fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
        } else if (mOrientationValues[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) {
            fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff *
                    (mOrientationValues[2] + 2.0 * Math.PI)
            );
            fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
        } else {
            fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * mOrientationValues[2];
        }
        //   Log.d(TAG, "calculateFusedOrientation: gyro:fused:" + fusedOrientation[0]);
        // overwrite gyro matrix and orientation with fused orientation to comensate gyro drift
        gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);

        System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
    }

    /*
     * Get the rotation matrix from the current orientation. Android Sensor
     * Manager does not provide a method to transform the orientation into a
     * rotation matrix, only the orientation from a rotation matrix. The basic
     * rotations can be found in Wikipedia with the caveat that the rotations
     * are *transposed* relative to what is required for this method.
     */
    private float[] getRotationMatrixFromOrientation(float[] orientation) {
        float[] xM = new float[9];
        float[] yM = new float[9];
        float[] zM = new float[9];

        float sinX = (float) Math.sin(orientation[1]);
        float cosX = (float) Math.cos(orientation[1]);
        float sinY = (float) Math.sin(orientation[2]);
        float cosY = (float) Math.cos(orientation[2]);
        float sinZ = (float) Math.sin(orientation[0]);
        float cosZ = (float) Math.cos(orientation[0]);

        // rotation about x-axis (pitch)
        xM[0] = 1.0f;
        xM[1] = 0.0f;
        xM[2] = 0.0f;
        xM[3] = 0.0f;
        xM[4] = cosX;
        xM[5] = sinX;
        xM[6] = 0.0f;
        xM[7] = -sinX;
        xM[8] = cosX;

        // rotation about y-axis (roll)
        yM[0] = cosY;
        yM[1] = 0.0f;
        yM[2] = sinY;
        yM[3] = 0.0f;
        yM[4] = 1.0f;
        yM[5] = 0.0f;
        yM[6] = -sinY;
        yM[7] = 0.0f;
        yM[8] = cosY;

        // rotation about z-axis (azimuth)
        zM[0] = cosZ;
        zM[1] = sinZ;
        zM[2] = 0.0f;
        zM[3] = -sinZ;
        zM[4] = cosZ;
        zM[5] = 0.0f;
        zM[6] = 0.0f;
        zM[7] = 0.0f;
        zM[8] = 1.0f;

        // Build the composite rotation... rotation order is y, x, z (roll, pitch, azimuth)
        float[] resultMatrix = matrixMultiplication(xM, yM);
        resultMatrix = matrixMultiplication(zM, resultMatrix);
        return resultMatrix;
    }
}
